#!/usr/bin/env python3
import sys
import rospy
import pygame
import numpy as np
import random 
import math
from turtle_pkg.msg import force_msg, velocity_msg, position_msg
from ascii_paint import *

class TurtleNode():
    def __init__(self):
        # init the ros 
        self.node = rospy.init_node("trutle")
        self.position_publisher = rospy.Publisher("position",position_msg,queue_size=1)
        self.velocity_publisher = rospy.Publisher("velocity",velocity_msg,queue_size=1)
        self.froce_subscriber = rospy.Subscriber("force", force_msg, callback=self.control_callback, queue_size=1)
        
        # init some param
        self.sizex = rospy.get_param("img_size_x")
        self.sizey = rospy.get_param("img_size_y")
        self.scale = rospy.get_param("img_scale")


        self.force = np.array([0.0, 0.0], np.float64)
        self.velocity = np.array([0.0, 0.0], np.float64)
        self.position = np.array([0.0, 0.0], np.float64)
        
        self.mass = rospy.get_param("trutle_mass")
        self.damping = rospy.get_param("trutle_damping")
        self.position[0] = rospy.get_param("init_pos_x")
        self.position[1] = rospy.get_param("init_pos_y")
        self.velocity[0] = rospy.get_param("init_vx")
        self.velocity[1] = rospy.get_param("init_vy")

        self.FPS = rospy.get_param("game_fps")

        self.img_path = rospy.get_param("/pkg_path/turtle_pkg")
        self.dis_threshold = rospy.get_param("dis_threshold")

        self.turtle_size = rospy.get_param("turtle_size")
        self.food_size = rospy.get_param("food_size")
        self.turtle_offset = self.turtle_size//2
        self.food_offset = self.food_size//2

    def control_callback(self,msg):
        ''''
        Modify the force
        '''
        self.force[0] = msg.x
        self.force[1] = msg.y

    def dis(self,x1,y1,x2,y2):
        return ((x1-x2)**2+(y1-y2)**2)**0.5

    def turtle(self):
        '''
        The main thread of the trutle, it subscirbe the controller topic,
        and publish the position msg for the further usage.
        '''

        #pygame init process
        pygame.init()
        screen = pygame.display.set_mode((self.sizex, self.sizey), pygame.DOUBLEBUF, 32)
        pygame.display.set_caption("turtle")
        clock = pygame.time.Clock()
        
        # load the resource to the pygame
        turtle = pygame.image.load(self.img_path+'/data/turtle')
        turtle = pygame.transform.scale(turtle,(self.turtle_size,self.turtle_size))
        food = pygame.image.load(self.img_path+'/data/hamburger.png')
        food = pygame.transform.scale(food,(self.food_size,self.food_size))
        
        # genrate the food
        foodList = rospy.get_param("food")

        # the time inteval
        dt = 1/self.FPS

        while True:
            # click 'x' to exit()
            for event in pygame.event.get():
                if event.type == pygame.QUIT:
                    exit()

            # move manager
            acceleration = (self.force - self.damping * self.velocity) / self.mass
            
            a = math.hypot(acceleration[0], acceleration[1])
            if a > 10:
                acceleration *= 10 / a
            self.position += self.velocity * dt + 0.5 * acceleration * dt**2
            self.velocity += acceleration * dt

            # range detection
            pix_x = self.position[0] * self.scale
            pix_y = self.sizey - self.position[1] * self.scale
            if(pix_x < self.turtle_offset or
               pix_x > self.sizex - self.turtle_offset or
               pix_y < self.turtle_offset or 
               pix_y > self.sizey - self.turtle_offset):
                rospy.logerr("turtle out of the screen")
                break

            # draw the trutle
            screen.fill((255, 255, 255))
            pix_x = self.position[0] * self.scale
            pix_y = self.sizey - self.position[1] * self.scale
            screen.blit(turtle, (pix_x-self.turtle_offset,pix_y-self.turtle_offset))
            

            # collison detection and draw the food
            is_finish = True
            for foodName, foodInfo in foodList.items():
                if math.hypot(self.position[0] - foodInfo['x'], self.position[1] - foodInfo['y']) < self.dis_threshold:
                    foodInfo['isEaten'] = True
                if(not foodInfo['isEaten']):
                    pix_x = foodInfo['x'] * self.scale
                    pix_y = self.sizey - foodInfo['y'] * self.scale
                    screen.blit(food, (pix_x-self.food_offset,pix_y-self.food_offset))
                    is_finish = False
            
            rospy.set_param("food", foodList)
            
            if(is_finish):
                finish()
                exit()
            
            # publish the msg
            pos_msg = position_msg()
            pos_msg.x = self.position[0]
            pos_msg.y = self.position[1]
            vel_msg = velocity_msg()
            vel_msg.x = self.velocity[0]
            vel_msg.y = self.velocity[1]
            self.position_publisher.publish(pos_msg)
            self.velocity_publisher.publish(vel_msg)
            pygame.display.update()
            time_passed = clock.tick(self.FPS)
        exit()



if __name__ == "__main__":
    br_team()
    t = TurtleNode()
    t.turtle()
